#ifndef FMatrix_H
#define FMatrix_H

#include <iostream>
#include <math.h>

#define O_mult_matrix(res, arg1,arg2) typeof(arg1 * arg2) res = arg1 * arg2

namespace Y{

template <int M, int N>
class FMatrix{

	public:

		float data[M][N];

	public:
		
		FMatrix () { }
		FMatrix ( float p[] ){
			for (int i = 0; i < M; ++i){
				for (int j = 0; j < N; ++j){
					data[i][j] = p[j * M + i];
				}
			}
		}
		FMatrix ( float p[M][N] ){
			for (int i = 0; i < M; ++i){
				for (int j = 0; j < N; ++j){
					data[i][j] = p[i][j];
				}
			}
		}
		FMatrix ( double p[M][N] ){
			for (int i = 0; i < M; ++i){
				for (int j = 0; j < N; ++j){
					data[i][j] = p[i][j];
				}
			}
		}
		
		FMatrix ( const FMatrix<M,N>& t2 ){
			for ( int i = 0; i < M; ++i ){ for (int j = 0; j < N; ++j){ data[i][j] = t2(i,j); } }
		}
		FMatrix<M,N>& operator= ( const FMatrix<M,N>& t2 ){
			for ( int i = 0; i < M; ++i ){ for (int j = 0; j < N; ++j){ data[i][j] = t2(i,j); } }
			return *this;
		}

		template <int P>
		FMatrix<M,P> operator* (const FMatrix<N,P>& m){
	
			FMatrix<M,P> nData;
			
			for (int i = 0; i < M; ++i){
				for (int j = 0; j < P; ++j){
					nData(i,j) = 0.0;
					for (int k = 0; k < N; ++k){
						nData(i,j) += m(k,j) * this->get(i,k);
					}
				}
			}
			
			return nData;
		}
		
		
		template <int P>
		FMatrix<M,P> operator* (float m[N][P]){
	
			FMatrix<M,P> nData;
			
			for (int i = 0; i < M; ++i){
				for (int j = 0; j < P; ++j){
					nData(i,j) = 0.0;
					for (int k = 0; k < N; ++k){
						nData(i,j) += m[k][j] * this->get(i,k);
					}
				}
			}
			
			return nData;
		}
				
		FMatrix& operator*= (float f){ for ( int i = 0; i < M; ++i ){ for ( int j = 0; j < N; ++j ){ data[i][j] *= f; } } }
		
		inline float get (int i, int j){ return data[i][j]; }
		inline float& operator() (int i, int j){ return data[i][j]; }
		inline float operator() (int i, int j) const{ return data[i][j]; }
		
		void print (){

			std::cout << "\n";
			for (int i = 0; i < M; ++i){
				for (int j = 0; j < N; ++j){
					std::cout << "| " << data[i][j] << " |";
				}
				std::cout << "\n";
			}

		}
		
		void makeIdentitFMatrix() { for ( int i = 0; i < M; ++i ){ for ( int j = 0; j < N; ++j ){ if ( i == j ){ data[i][j] = 1.0; } else{ data[i][j] = 0.0; } } } }
		void loadIdentity();
		void translate(float dx, float dy, float dz);
		void scale(float sx, float sy, float sz);
		void rotate_x(float deg);
		void rotate_y(float deg);
		void rotate_z(float deg);
		void negate_x();
		void negate_y();
		void negate_z();
};


		template <int M, int N, int P>
		static inline void mult (FMatrix<M,P>& nData,   const FMatrix<M,N>& m1 , const FMatrix<N,P>& m){
			
			float buf[M][P] = {0.0};
			for (int i = 0; i < M; ++i){
				for (int j = 0; j < P; ++j){
					for (int k = 0; k < N; ++k){
						buf[i][j] += m.data[k][j] * m1.data[i][k];
					}
				}
			}
			
			for ( int i = 0; i < M; ++i ){
				for ( int j = 0; j < P; ++j ){
					nData.data[i][j] = buf[i][j];
				}
			}

		}
		
		template <int M, int N, int P>
		static inline void mult (FMatrix<M,P>& nData,   const FMatrix<M,N>& m1 , float m[N][P]){
			
			float buf[M][P] = {0.0};
			for (int i = 0; i < M; ++i){
				for (int j = 0; j < P; ++j){
					for (int k = 0; k < N; ++k){
						buf[i][j] += m[k][j] * m1.data[i][k];
					}
				}
			}
			
			for ( int i = 0; i < M; ++i ){
				for ( int j = 0; j < P; ++j ){
					nData.data[i][j] = buf[i][j];
				}
			}

		}
		
		template <int M, int N, int P>
		static inline void mult (FMatrix<M,P>& nData,   float m1[M][N], const FMatrix<N,P>& m){
			
			float buf[M][P] = {0.0};
			for (int i = 0; i < M; ++i){
				for (int j = 0; j < P; ++j){
					for (int k = 0; k < N; ++k){
						buf[i][j] += m.data[k][j] * m1[i][k];
					}
				}
			}
			
			for ( int i = 0; i < M; ++i ){
				for ( int j = 0; j < P; ++j ){
					nData.data[i][j] = buf[i][j];
				}
			}

		}


template<int M,int N>
void FMatrix<M,N>::loadIdentity (){
	for ( int i = 0; i < M; ++i ){	
		for ( int j = 0; j < N; ++j ){
			if ( i == j )	{ ((*this))(i,j) = 1.0; }
			else			{ ((*this))(i,j) = 0.0; }
		}
	}
}

template<int M,int N>
void FMatrix<M,N>::translate (float dx, float dy, float dz)
{
	float trans[4][4] = {	{ 1.0 , 0.0 , 0.0 , dx	},
							{ 0.0 , 1.0 , 0.0 , dy	},
							{ 0.0 , 0.0 , 1.0 , dz	},
							{ 0.0 , 0.0 , 0.0 , 1.0			}	};
	
	mult( (*this),		(*this),trans );
}

template<int M,int N>
void FMatrix<M,N>::scale (float sx, float sy, float sz)
{
	float scale[4][4] = {	{ sx , 0.0 , 0.0 , 0.0	},
							{ 0.0 , sy , 0.0 , 0.0	},
							{ 0.0 , 0.0 , sz , 0.0	},
							{ 0.0 , 0.0 , 0.0 , 1.0	}	};
	
	mult( (*this),		(*this),scale );
}

template<int M,int N>
void FMatrix<M,N>::rotate_x (float degrees) {
	float radians = (degrees / 360.0) * 2 * 3.14159;
	float rotate[4][4] = {	{ 1.0 , 0.0 , 0.0 , 0.0	},
							{ 0.0 , cos(radians) , -1.0 * sin(radians) , 0.0	},
							{ 0.0 , sin(radians) , cos(radians) , 0.0	},
							{ 0.0 , 0.0 , 0.0 , 1.0	}	};
							
	mult( (*this),		(*this),rotate );
}

template<int M,int N>
void FMatrix<M,N>::rotate_y (float degrees) {
	float radians = (degrees / 360.0) * 2 * 3.14159;
	float rotate[4][4] = {	{ cos(radians) , 0.0 , sin(radians) , 0.0	},
							{ 0.0 , 1.0 , 0.0 , 0.0	},
							{ -1.0 * sin(radians) , 0.0 , cos(radians) , 0.0	},
							{ 0.0 , 0.0 , 0.0 , 1.0	}	};
	
	mult( (*this),		(*this),rotate );
}

template<int M,int N>
void FMatrix<M,N>::rotate_z (float degrees) {
	float radians = (degrees / 360.0) * 2 * 3.14159;
	float rotate[4][4] = {	{ cos(radians) , -1.0 * sin(radians) , 0.0 , 0.0	},
							{ sin(radians) , cos(radians) , 0.0 , 0.0	},
							{ 0.0 , 0.0, 1.0 , 0.0	},
							{ 0.0 , 0.0 , 0.0 , 1.0	}	};
	
	mult( (*this),		(*this),rotate );
}

template<int M,int N>
void FMatrix<M,N>::negate_x () {
	
	float neg[4][4] = {	{ -1.0 , 0.0 , 0.0 ,  0.0	},
							{ 0.0 , 1.0 , 0.0 , 0.0		},
							{ 0.0 , 0.0 , 1.0 , 0.0		},
							{ 0.0 , 0.0 , 0.0 ,  1.0	}	};
	mult((*this),	(*this),neg);
}

template<int M,int N>
void FMatrix<M,N>::negate_y () {

	float neg[4][4] = {	{ 1.0 , 0.0 , 0.0 ,  0.0	},
							{ 0.0 , -1.0 , 0.0 ,  0.0	},
							{ 0.0 , 0.0 , 1.0 , 0.0	},
							{ 0.0 , 0.0 , 0.0 ,  1.0	}	};
	mult((*this),	(*this),neg);

}

template<int M,int N>
void FMatrix<M,N>::negate_z () {
	float neg[4][4] = {	{ 1.0 , 0.0 , 0.0 ,  0.0	},
							{ 0.0 , 1.0 , 0.0 , 0.0	},
							{ 0.0 , 0.0 , -1.0 , 0.0		},
							{ 0.0 , 0.0 , 0.0 ,  1.0	}	};
	mult((*this),	(*this),neg);
}


}
#endif